#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/distance_sensor.h>
#include <stdio.h>

#define TIME_STEP 64


//瀵よ埖妞傞崙鑺ユ殶閻ㄥ嫪绶ョ€?
void delayTimes(int step)
{
  for (int i = 0; i < step; ++i) {
    
    wb_robot_step(wb_robot_get_basic_time_step());  
  }
}


int main(int argc, char **argv) {
  wb_robot_init();

  WbDeviceTag ds[5];
  char ds_names[5][15] = {"ds_left", "ds_right","ds_main","ds_left(1)", "ds_right(1)"};
  for (int i = 0; i < 5; i++) {
    ds[i] = wb_robot_get_device(ds_names[i]);
    wb_distance_sensor_enable(ds[i], TIME_STEP);
  }


  WbDeviceTag motors[4];
  char motors_names[4][8] = {"wheel1", "wheel2", "wheel3", "wheel4"};
  for (int i = 0; i < 4; i++) {
    motors[i] = wb_robot_get_device(motors_names[i]);
    wb_motor_set_position(motors[i], INFINITY);
    wb_motor_set_velocity(motors[i],0);
  }


  double left_speed = 1.0;
  double right_speed = 1.0;
  int flag=0;

  while (wb_robot_step(TIME_STEP) != -1) {
    
    float lf_wheel = wb_distance_sensor_get_value(ds[0]);
    float rt_wheel = wb_distance_sensor_get_value(ds[1]);
    float ma_wheel = wb_distance_sensor_get_value(ds[2]);
    float lf1_wheel = wb_distance_sensor_get_value(ds[3]);
    float rt1_wheel = wb_distance_sensor_get_value(ds[4]);
    
    printf("left(1):%f ,left:%f ,main:%f ,right:%f right(1):%f flag:%d\n",lf1_wheel,lf_wheel,ma_wheel,rt_wheel,rt1_wheel,flag);
    if(lf1_wheel<=51&&lf_wheel<51&&ma_wheel>=51&&rt_wheel<=51&&rt1_wheel<=51)
            {left_speed = 1.0;right_speed = 1.0;}//00100
     if(flag==9&&lf1_wheel<=51&&lf_wheel<=51&&ma_wheel<=51&&rt_wheel<=51&&rt1_wheel>=51)
     {left_speed = 1.0;right_speed = 0.0;}//00001
     
     
    if(lf1_wheel<=51&&lf_wheel<=51&&ma_wheel<=51&&rt_wheel>=51&&rt1_wheel<=51)
     {left_speed = 1.0;right_speed = 0.0;}//00010
     
    if(lf1_wheel<=51&&lf_wheel>=51&&ma_wheel<=51&&rt_wheel<=51&&rt1_wheel<=51)
        {left_speed = 0.0;right_speed = 1.0;} //01000
        
    if(lf1_wheel<=51&&lf_wheel<=51&&ma_wheel>=51&&rt_wheel>=51&&rt1_wheel<=51)
            {left_speed = 1.0;right_speed = 0.0;}//00110
            
     if(lf1_wheel<=51&&lf_wheel>=51&&ma_wheel>=51&&rt_wheel>=51&&rt1_wheel<=51)
            {left_speed = 1.0;right_speed = 0.0;}//01110
            
      if(lf1_wheel<=51&&lf_wheel>=51&&ma_wheel<=51&&rt_wheel>=51&&rt1_wheel<=51)
            {left_speed = 1.0;right_speed = 0.0;}//01010   
               
    if(lf1_wheel<=51&&lf_wheel>=51&&ma_wheel>=51&&rt_wheel<=51&&rt1_wheel<=51)
            {left_speed = 0.0;right_speed = 1.0;}//01100
            
    
    if(lf1_wheel>=51&&lf_wheel>=51&&ma_wheel>=51&&rt_wheel<=51&&rt1_wheel<=51)
            {left_speed = 0.0;right_speed = 2.0;}//11100
            
    if(lf1_wheel<=51&&lf_wheel<51&&ma_wheel>=51&&rt_wheel>=51&&rt1_wheel>=51)
            {left_speed = 2.0;right_speed = 0.0;}//00111
            
                
     if(flag==7&&lf1_wheel>=51&&lf_wheel>=51&&ma_wheel>=51&&rt_wheel>=51&&rt1_wheel>=51)
            {left_speed = 1.0;right_speed = 1.0;delayTimes(150);
            left_speed = -1.0;right_speed = -1.0;delayTimes(100);
             flag++;} //11111  
             
             
     if(flag==8&&lf1_wheel<=51&&lf_wheel<=51&&ma_wheel>=51&&rt_wheel<=51&&rt1_wheel<=51)
            {
            left_speed = -1.0;right_speed = -1.0;delayTimes(500);flag++;
            }   
            
       if(flag<7&&lf1_wheel>=51&&lf_wheel>=51&&ma_wheel>=51&&rt_wheel>=51&&rt1_wheel>=51)
            {left_speed = 1.0;right_speed = 0.0;flag++;} //11111
            
       if(flag>=9&&lf1_wheel>=51&&lf_wheel>=51&&ma_wheel>=51&&rt_wheel>=51&&rt1_wheel>=51)
            {left_speed = 1.0;right_speed = 1.0;} //11111
            
        if(flag>=9&&lf1_wheel<=51&&lf_wheel>=51&&ma_wheel>=51&&rt_wheel>=51&&rt1_wheel>=51)
            {left_speed = 1.0;right_speed = 1.0;} //11111
            
       
   if(lf1_wheel<=51&&lf_wheel<51&&ma_wheel>=51&&rt_wheel<=51&&rt1_wheel<=51)
            {left_speed = 1.0;right_speed = 1.0;}//00101
      
    wb_motor_set_velocity(motors[0],left_speed);
    wb_motor_set_velocity(motors[1],right_speed);
    wb_motor_set_velocity(motors[2],left_speed);
    wb_motor_set_velocity(motors[3],right_speed);  
 
    
    //delayTimes(300);
    //left_speed = 0.0;
    //right_speed = 0.0;  
    
  }

  wb_robot_cleanup();
  return 0;
}